//
//  astar.c
//  intmap
//
//  Created by liwei on 13-11-10.
//  Copyright (c) 2013年 liwei. All rights reserved.
//
#include "astar.h"
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <memory.h>

int find_most_nearest_neigh(VALUE data[], L length, X x, X y)
{
	INDEX index;
	INDEX current;
	L number;
	L median;
    
	if(NULL == data || 0 == length)
		return -1;
    
	current = 0;
	number = (L) sqrt((data[0].x - x) * (data[0].x - x)+ (data[0].y - y) *  (data[0].y - y));
    
	for(index = 1; index < length; index ++){
		median = (L) sqrt((data[index].x - x) * (data[index].x - x)+ (data[index].y - y) *  (data[index].y - y));
		
		if(median < number){
			number = median;
			current = index;
		}
	}
    
	return current;
}

int check_pos_valid(X x, Y y)
{
    return (0 <= x < 5) && (0 <= y <5);
}

VALUE* updata_data_for_queue(VALUE* data, L length, L* new_len)
{
	int index;
	int count;
	int max;
	VALUE* p_data;
    
	if(NULL == data || 0 == length || NULL == new_len)
		return NULL;
    
	max = length << 2;
	p_data = (VALUE*)malloc(max * sizeof(VALUE));
	memset(p_data, 0, max * sizeof(VALUE));
    
	count = 0;
	for(index = 0; index < length; index ++){
		if(check_pos_valid(data[index].x, data[index].y - 1)){
			p_data[count].x = data[index].x;
			p_data[count].y = data[index].y -1;
			count ++;
		}
        
		if(check_pos_valid(data[index].x -1, data[index].y)){
			p_data[count].x = data[index].x -1;
			p_data[count].y = data[index].y;
			count ++;
		}
        
		if(check_pos_valid(data[index].x, data[index].y + 1)){
			p_data[count].x = data[index].x;
			p_data[count].y = data[index].y +1;
			count ++;
		}
        
		if(check_pos_valid(data[index].x + 1, data[index].y)){
			p_data[count].x = data[index].x + 1;
			p_data[count].y = data[index].y;
			count ++;
		}
	}
    
	*new_len = count;
	return p_data;
}

void find_path(VALUE* data, VALUE origin, VALUE target)
{
    INDEX nearest = 0;
    VALUE *p_data = NULL;
    L length = LENGTH_ORIGIN;
    L new_len = 0;
    L distance = (L) sqrt((origin.x - target.x)*(origin.x - target.x) + (origin.y - target.y)*(origin.y - target.y));
    /* 循环条件: 最短距离不为0 */
    while(distance){
        /* 更新列表 */
        p_data = updata_data_for_queue(data, length, &new_len);
        length = new_len;
        
        /* 寻找最近点 */
        nearest = find_most_nearest_neigh(p_data, length, target.x, target.y);
        if (!p_data) {
            continue;
        }
        distance = (L) sqrt((p_data[nearest].x - target.x)*(p_data[nearest].x - target.x) + (p_data[nearest].y - target.y)*(p_data[nearest].y - target.y));
        printf("%d %d", p_data[nearest].x, p_data[nearest].y);
        if (p_data) {
            free(p_data);
        }
    };
}


